#include <behaviortree_cpp_v3/behavior_tree.h>
#include <behaviortree_cpp_v3/bt_factory.h>
using namespace std;

BT::NodeStatus checkBattery()
{
    cout << "Battery is OK" << endl;
    return BT::NodeStatus::SUCCESS;
}

struct Pose2D
{
    double x, y, theta;
};

class MoveBaseAction : public BT::SyncActionNode
{
private:
    int count_ = 0;
public:
    MoveBaseAction(const string& name, const BT::NodeConfiguration& config)
        : BT::SyncActionNode(name, config)
    {
    }

    static BT::PortsList providedPorts()
    {
        return {
            BT::InputPort<double>("x"),
            BT::InputPort<double>("y"),
            BT::InputPort<double>("z"),
        };
    }

    BT::NodeStatus tick() override
    {
        Pose2D goal;
        if (!getInput<double>("x", goal.x) || !getInput<double>("y", goal.y) || !getInput<double>("z", goal.theta))
        {
            throw BT::RuntimeError("Missing required input [goal]");
        }
        printf("Moving to goal: x:%f, y:%f, theta:%f\n", goal.x, goal.y, goal.theta);
        while (count_++ < 5)
        {
            this_thread::sleep_for(chrono::seconds(1));
            // return BT::NodeStatus::RUNNING;
        }
        printf("MoveBaseAction finished\n");
        return BT::NodeStatus::SUCCESS;
    }
};

class SaySomething : public BT::SyncActionNode
{
public:
    SaySomething(const string& name, const BT::NodeConfiguration& config)
        : BT::SyncActionNode(name, config)
    {
    }

    BT::NodeStatus tick() override
    {
        auto message = getInput<string>("message");
        if (!message)
        {
            return BT::NodeStatus::FAILURE;
        }

        cout << this->name() << ":" << message.value() << endl;
        return BT::NodeStatus::SUCCESS;
    }

    static BT::PortsList providedPorts()
    {
        return { BT::InputPort<string>("message") };
    }
};

static const char* xml_text_sequence = R"(

 <root main_tree_to_execute = "MainTree" >

     <BehaviorTree ID="MainTree">
        <Sequence name="root">
            <BatteryOK/>
            <SaySomething   message="mission started..." />
            <MoveBase       x="1" y="2" z="3" />
            <SaySomething   message="mission completed!" />
        </Sequence>
     </BehaviorTree>

 </root>
 )";

static const char* xml_text_reactive = R"(

 <root main_tree_to_execute = "MainTree" >

     <BehaviorTree ID="MainTree">
        <ReactiveSequence name="root">
            <BatteryOK/>
            <Sequence>
                <SaySomething   message="mission started..." />
                <MoveBase       x="1" y="2" z="3" />
                <SaySomething   message="mission completed!" />
            </Sequence>
        </ReactiveSequence>
     </BehaviorTree>

 </root>
 )";

int main()
{
    BT::BehaviorTreeFactory factory;
    factory.registerSimpleCondition("BatteryOK", bind(checkBattery));
    factory.registerNodeType<MoveBaseAction>("MoveBase");
    factory.registerNodeType<SaySomething>("SaySomething");

    for (auto& xml : { xml_text_sequence, xml_text_reactive })
    {
        auto tree = factory.createTreeFromText(xml);
        BT::NodeStatus status = tree.tickRoot();
        cout << "tree.tickRoot() status=" << BT::toStr(status) << endl;

        while (status == BT::NodeStatus::RUNNING)
        {
            tree.sleep(std::chrono::milliseconds(100));
            std::cout << "--- ticking\n";
            status = tree.tickRoot();
            std::cout << "--- status: " << toStr(status) << "\n\n";
        }
    }

    return 0;
}